#include <stdlib.h>
#include "ThreePositionSwitch.h"
#include "DriverStation.h"

bool
ThreePositionSwitch::isNearEnough(int rawValue, int threshold)
{
	return (abs(rawValue - threshold) <= kThresholdLeeway);
}

ThreePositionSwitch::ThreePositionSwitch(unsigned int channel, unsigned int id) : Switch(channel, id)
{
	//There's nothing to do here... Super is a jerk!
}

ThreePositionSwitch::Position
ThreePositionSwitch::GetPosition(void)
{
	int rawValue = (int)DriverStation::GetInstance()->GetAnalogIn(m_channel);
	
	if(isNearEnough(rawValue, kPositivePositionThreshold))
		return Positive;
	else if(isNearEnough(rawValue, kNeutralPositionThreshold))
		return Neutral;
	else if(isNearEnough(rawValue, kNegativePositionThreshold))
		return Negative;
	else
		throw -1;	
}
